#include <stdio.h>
#include "Motor.h"
#include "freertos/FreeRTOS.h"
#include "string.h"
#define RESET 0
#define NOT_RESET 1
#define MOTOR_ID_1 0x00
#define MOTOR_ID_2 0x01

static uint8_t _watchDogState[2] = {0}; //0: not reset, 1: reset
Motor motor1;
Motor motor2;

void MotorInit(void) {
    motor1.NodeID = MOTOR_ID_1;
    motor2.NodeID = MOTOR_ID_2;
    //检查电机参数 让电机发出声音
    // uint8_t data[8] = {0x04,0x00,0x00,0x00,0x00,0x00,0x00,0x00};
    // // CanSendMessage(motor1.NodeID,MSG_SET_AXIS_REQUESTED_STATE,data,8);
    // // vTaskDelay(pdMS_TO_TICKS(5000));
    // // //编码器校准 电机会旋转
    // // data[0] = 0x07;
    // // CanSendMessage(motor1.NodeID,MSG_SET_AXIS_REQUESTED_STATE,data,8);
    // // vTaskDelay(pdMS_TO_TICKS(3000));
    // //进入轴闭环模式
    // data[0] = 0x08;
    // CanSendMessage(motor1.NodeID,MSG_SET_AXIS_REQUESTED_STATE,data,8);
    // vTaskDelay(pdMS_TO_TICKS(1000));
    // //设置控制器模式，力矩模式 - 直接输入
    // data[0] = 0x01;
    // data[4] = 0x01;
    // CanSendMessage(motor1.NodeID,MSG_SET_CONTROLLER_MODES,data,8);
    // vTaskDelay(pdMS_TO_TICKS(10));
    // SetMotorLimit(0,10,20);
    // vTaskDelay(pdMS_TO_TICKS(10));
}
void SetMotorLimit(uint8_t motorIndex,float velLimit,float currentSoftMax) {
    union Limit
    {  
        struct 
        {
            float VelLimit;
            float CurrentLimit;
        } Limit;
        uint8_t data[8];
    } limitData;
    memset(limitData.data,0,8);
    limitData.Limit.VelLimit = velLimit;
    limitData.Limit.CurrentLimit = currentSoftMax;
    if(motorIndex == 0){
        CanSendMessage(motor1.NodeID,MSG_SET_INPUT_TORQUE,limitData.data,8);
    }
    else if(motorIndex == 1){
        CanSendMessage(motor2.NodeID,MSG_SET_INPUT_TORQUE,limitData.data,8);
    }
}
void SetMotorTorque(uint8_t motorIndex,float torque) {
    union Torque
    {  
        float torque;
        uint8_t data[8];
    } torqueData; //局部变量记得初始化，否则会出现未知错误
    memset(torqueData.data,0,8);
    torqueData.torque = torque;
    if(motorIndex == 0){
        CanSendMessage(motor1.NodeID,MSG_SET_INPUT_TORQUE,torqueData.data,8);
    }
    else if(motorIndex == 1){
        CanSendMessage(motor2.NodeID,MSG_SET_INPUT_TORQUE,torqueData.data,8);
    }
}
double GetMotorPos(uint8_t motorIndex) {
    if(motorIndex == 0){
        return motor1.Position;
    }else if(motorIndex == 1){
        return motor2.Position;
    }
    return 0.0;
} 
double GetMotorSpeed(uint8_t motorIndex) {
    if(motorIndex == 0){
        return motor1.Speed;
    }else if(motorIndex == 1){
        return motor2.Speed;
    }
        return 0;
}
void ResetMotortWatchDog(uint8_t MotorIndex) {
    _watchDogState[MotorIndex] = NOT_RESET;
}
void SetMotortWatchDogReady(uint8_t MotorIndex) {
    _watchDogState[MotorIndex] = RESET;
}
void ResetEncoder(){
    // union Torque
    // {  
    //     struct{

    //     }float pos;
    //     int16_t Vel_FF;
    //     int16_t Torque_FF;
        
    //     uint8_t data[8];
    // } Data; //局部变量记得初始化，否则会出现未知错误
    //uint8_t data[8] = {0};
    //CanSendMessage(motor1.NodeID,MSG_SET_INPUT_POS,data,8);
}
uint8_t GetWatchDogState(uint8_t MotorIndex){
    return _watchDogState[MotorIndex];
}